#include <iostream>
#include <cmath>
#include <random>
#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include "std_msgs/Bool.h"
#include "std_msgs/Empty.h"
#include "std_msgs/Float32.h"

#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/organized.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/conditional_removal.h>

using namespace Eigen;

typedef pcl::PointXYZ VPoint;
typedef pcl::PointCloud<VPoint> VPointCloud;

ros::Publisher world_lidar_pub;

Matrix4d lidar2body;
Matrix4d body2world;

std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> dis(0.0, 1.0);  

void OriginLidarCallback(const sensor_msgs::PointCloud2ConstPtr& mmwCloudMsg) {    
  // to pcl
  boost::shared_ptr<VPointCloud> originCloud = boost::make_shared<VPointCloud>();
  //pcl::fromROSMsg(laserCloudMsg, *originCloud);
  pcl::fromROSMsg(*mmwCloudMsg, *originCloud);
  // to body
  boost::shared_ptr<VPointCloud> bodyCloud = boost::make_shared<VPointCloud>();
  pcl::transformPointCloud(*originCloud, *bodyCloud, lidar2body);
  // crop
  pcl::CropBox<VPoint> crop;
  crop.setInputCloud(bodyCloud);
  crop.setMin(Eigen::Vector4f(-0.750, -0.550, -0.100, 1.0));
  crop.setMax(Eigen::Vector4f(0.750, 0.550, 0.600, 1.0));
  crop.setNegative(true); // outside
  crop.filter(*bodyCloud);
  // near land
  pcl::ConditionOr<VPoint>::Ptr range_cond(new pcl::ConditionOr<VPoint>());
  range_cond->addComparison(pcl::FieldComparison<VPoint>::ConstPtr(
      new pcl::FieldComparison<VPoint>("z", pcl::ComparisonOps::LT, -0.20)));  // z < -0.05
  range_cond->addComparison(pcl::FieldComparison<VPoint>::ConstPtr(
      new pcl::FieldComparison<VPoint>("z", pcl::ComparisonOps::GT, 0.20)));   // z > 0.05

  pcl::ConditionalRemoval<VPoint> cond_removal;
  cond_removal.setCondition(range_cond);
  cond_removal.setInputCloud(bodyCloud);
  cond_removal.setKeepOrganized(false);  // 不保留结构（适用于无序点云）
  cond_removal.filter(*bodyCloud);
  // to world
  boost::shared_ptr<VPointCloud> worldCloud = boost::make_shared<VPointCloud>();
  pcl::transformPointCloud(*bodyCloud, *worldCloud, body2world);
  // to msg
  sensor_msgs::PointCloud2 lidarPointInWorld;
  pcl::toROSMsg(*worldCloud, lidarPointInWorld);
  lidarPointInWorld.header.stamp = mmwCloudMsg->header.stamp;
  lidarPointInWorld.header.frame_id = "world";
  world_lidar_pub.publish(lidarPointInWorld);
}  

void OdomCallback(const nav_msgs::OdometryConstPtr& msg) {
	Matrix4d Pose_receive = Matrix4d::Identity();

  Eigen::Vector3d request_position;
  Eigen::Quaterniond request_pose;
  request_position.x() = msg->pose.pose.position.x;
  request_position.y() = msg->pose.pose.position.y;
  request_position.z() = msg->pose.pose.position.z;
  request_pose.x() = msg->pose.pose.orientation.x;
  request_pose.y() = msg->pose.pose.orientation.y;
  request_pose.z() = msg->pose.pose.orientation.z;
  request_pose.w() = msg->pose.pose.orientation.w;
  Pose_receive.block<3, 3>(0, 0) = request_pose.toRotationMatrix();
  Pose_receive(0, 3) = request_position(0);
  Pose_receive(1, 3) = request_position(1);
  Pose_receive(2, 3) = request_position(2);
  body2world = Pose_receive;
}

int main(int argc, char **argv) {
  // 随机数

  // 初始化ROS节点  
  ros::init(argc, argv, "simulator_adapter_node");
  // 创建节点句柄  
  ros::NodeHandle n("~");
  // 获取外参
  double rollDegree, pitchDegree, yawDegree;
  double x, y, z;
  n.param("lidar/rollDegree", rollDegree, 0.0);
  n.param("lidar/pitchDegree", pitchDegree, 0.0);
  n.param("lidar/yawDegree", yawDegree, 0.0);
  n.param("lidar/x", x, 0.0);
  n.param("lidar/y", y, 0.0);
  n.param("lidar/z", z, 0.0);
  //ROS_INFO("Param, %f, %f", rollDegree, z);

  
  // 初始化传感器外参
  lidar2body << 1.0, 0.0, 0.0, x,
  									0.0, 1.0, 0.0, y,
  									0.0, 0.0, 1.0, z,
  									0.0, 0.0, 0.0, 1.0;
  double rollRad = rollDegree * M_PI / 180.0;  
  double pitchRad = pitchDegree * M_PI / 180.0;  
  double yawRad = yawDegree * M_PI / 180.0;  
  
  // 创建从RPY得到的旋转矩阵  
  Eigen::AngleAxisd rollAngle(rollRad, Eigen::Vector3d::UnitX());  
  Eigen::AngleAxisd pitchAngle(pitchRad, Eigen::Vector3d::UnitY());  
  Eigen::AngleAxisd yawAngle(yawRad, Eigen::Vector3d::UnitZ());  
  Eigen::Matrix3d rotation = yawAngle.toRotationMatrix() * pitchAngle.toRotationMatrix() * rollAngle.toRotationMatrix();  
  lidar2body.block<3, 3>(0, 0) = rotation;
  
  // 创建一个订阅者，订阅名为chatter的话题，队列长度为1000  
  ros::Subscriber origin_lidar_sub = n.subscribe<sensor_msgs::PointCloud2>("origin_lidar_topic", 1, OriginLidarCallback);
  ros::Subscriber odom_sub = n.subscribe<nav_msgs::Odometry>("odom_topic", 1, OdomCallback);

  // ros::Subscriber accSub = n.subscribe<geometry_msgs::TwistStamped>("/drone1/CERLAB/quadcopter/acc", 1, AccCallback);
  // ros::Subscriber velSub = n.subscribe<geometry_msgs::TwistStamped>("/drone1/CERLAB/quadcopter/vel", 1, VelCallback);
  
  world_lidar_pub = n.advertise<sensor_msgs::PointCloud2>("world_lidar_topic", 1);

  // 循环等待回调函数  
  ros::spin();  
  
  return 0;  

}
